
#include <stddef.h>
#include <stdio.h>
#include <stdint.h>

#include "mcu.h"
#include "../mcu/inc/usart.h"
#include "i2c.h"
#include "../mcu/inc/extint.h"
#include "../hw/inc/hmc5883l.h"
#include "../mcu/inc/pwm_out.h"
#include "../hw/inc/ads1100.h"
#include "header.h"
#include <math.h>




/**
 **===========================================================================
 **
 **  Abstract: main program
 **
 **===========================================================================
 */

int tick = 0;
int counter = 0;
float	DESIRED = 0;			//wanted azimuth heading
int desiredFlag = 0;

void handleReceivedChar(unsigned char data)
{

}


void Wait(uint32_t Time)
{
	while(Time--)asm("nop\n\r");

}


int main(void)
{
	volatile float height = 0;					//computed height
	float pressf = 0;
	unsigned int press;
	char heights[20];
	char pressfs[20];

	initUSART1();					//inits USART1
	initI2C1();						//inits I2C
	initGPIO();						//inits PB7 for user LED
	EXTI0_Config();					//inits interrupt from user button
	EXTI1_Config();					//inits interrupt from compass DATARDY
	initPWM_Output();				//inits TIM3 for PWM configuration for RGB LED driving
	initADS1100();					//inits ADS1100 AD converter - pressure sensor

	RegisterCallbackUART1(&handleReceivedChar);
	PutsUART1("start");

	while (1)
	{
		readDataADS1100(&press);
		delay_us(30000);
		userLed(1);
		delay_us(10000);
		userLed(0);
		writeByteI2C1(compassModuleAddress,modeRegister,singleMeasurementMode);			//prompts compass module for readings
		pressf = (float)press * 0.03841717105;											//converts data from ADS1100 to pressure in hPa
		height = heightFromPressure(pressf-14, 1013.25);									//prompts heightFromPressure function for returning real height from pressure info
		sprintf(heights,"%.2f",height);													//converts height from float to string
		sprintf(pressfs,"%.3f",pressf);													//converts height from float to string
		PutcUART1(ASTERISK);															//sends * character for packet start recognition
		PutsUART1(pressfs);																//sends actual height value
		PutcUART1(0);																	//end of partial info delimiter "\0"
		PutsUART1(heights);																//sends actual height value
		PutcUART1(0);																	//end of partial info delimiter "\0"
	}

	return 0;
}


	void initGPIO(void)					//initialisation of GPIO pin PB7, where is connected user LED
	{
	GPIO_InitTypeDef GPIO_InitStructure;

	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_40MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	}

	void userLed(unsigned char level)		//function for turning on/off the LED
	{
		switch(level)
		{
		case 1:
		GPIOB->BSRRL = GPIO_Pin_7;
		break;

		case 0:
		GPIOB->BSRRH = GPIO_Pin_7;
		break;
		}
	}

	void interruptHandlerRoutine()
	{
		unsigned char	XMSB;					//MSB for X direction
		unsigned char	XLSB;					//LSB for X direction
		unsigned char	YMSB;					//MSB for Y direction
		unsigned char	YLSB;					//LSB for Y direction
		unsigned char	ZMSB;					//MSB for Z direction
		unsigned char	ZLSB;					//LSB for Z direction

		char	orientationS[10];				//buffer for converting int to string via sprintf

		int	DEVIATION = 0;							//deviation between DESIRED and actual

		volatile int16_t Xorientation;			//computed X orientation
		volatile int16_t Yorientation;			//computed Y orientation
		volatile int16_t Zorientation;			//computed Z orientation

		volatile int16_t XcorrectionA = 140;			//computed aditive X correction
		volatile int16_t YcorrectionA = 20;			//computed aditive Y correction
		//volatile int16_t ZcorrectionA;			//computed aditive Z correction

		volatile int16_t XcorrectionM = 1;			//computed multiplicative X correction
		volatile int16_t YcorrectionM = 1.1;			//computed multiplicative Y correction
		//volatile int16_t ZcorrectionM;			//computed multiplicative Z correction

		char  XorientationS[10];				//buffer for converting int to string via sprintf
		char  YorientationS[10];				//buffer for converting int to string via sprintf
		char  ZorientationS[10];				//buffer for converting int to string via sprintf

		//volatile float azimut;					//heading azimuth
		volatile float orientation;
		float inclination = 3.71;

		//THIS SECTION READS ACTUAL DATA FROM COMPASS MODULE
		readByteI2C1(compassModuleAddress,dataOutputXLSBRegister,&XLSB);
		readByteI2C1(compassModuleAddress,dataOutputXMSBRegister,&XMSB);
		readByteI2C1(compassModuleAddress,dataOutputYLSBRegister,&YLSB);
		readByteI2C1(compassModuleAddress,dataOutputYMSBRegister,&YMSB);
		readByteI2C1(compassModuleAddress,dataOutputZLSBRegister,&ZLSB);
		readByteI2C1(compassModuleAddress,dataOutputZMSBRegister,&ZMSB);

		//PARSING 8bit CHARS FROM COMPASS MODULE TO INTEGER 16bits
		Xorientation = ((XMSB << 8) + XLSB);
		Yorientation = ((YMSB << 8) + YLSB);
		Zorientation = ((ZMSB << 8) + ZLSB);

		///orientation = sqrt(pow(Xorientation+190,2.0) + pow(Yorientation,2.0)) + inclination;			//computed orientation as sqare root of sum of power 2 of Xorientation and power 2 of Y orientation
		orientation = atan2((Yorientation*YcorrectionM + YcorrectionA),(Xorientation*XcorrectionM + XcorrectionA))*100 + inclination;
		//azimut = ((359*orientation)/(2048));										//azimuth estimation

		if(orientation < 0)
			orientation += 2*PI;

		  // Check for wrap due to addition of declination.
		  if(orientation > 2*PI)
			  orientation -= 2*PI;


		sprintf(orientationS,"%.0f",orientation);					//converting float to string - azimuth
		PutsUART1(orientationS);								//send azimuth info via UART
		PutcUART1(0);
		sprintf(XorientationS,"%i",Xorientation*XcorrectionM + XcorrectionA);				//converting float to string - Xorientation
		PutsUART1(XorientationS);								//send azimuth info via UART
		PutcUART1(0);
		sprintf(YorientationS,"%i",Yorientation*YcorrectionM + YcorrectionA);				//converting float to string - Yorientation
		PutsUART1(YorientationS);								//send azimuth info via UART
		PutcUART1(0);
		sprintf(ZorientationS,"%i",Zorientation);				//converting float to string - Zorientation
		PutsUART1(ZorientationS);								//send azimuth info via UART

		PutcUART1(10);

		DEVIATION = fabs(DESIRED - (int)orientation);

		if(DEVIATION > 45)	rgbLedColor(c1);
		if((DEVIATION > 40) && (DEVIATION < 45))	rgbLedColor(c2);
		if((DEVIATION > 35) && (DEVIATION < 40))	rgbLedColor(c3);
		if((DEVIATION > 30) && (DEVIATION < 35))	rgbLedColor(c4);
		if((DEVIATION > 25) && (DEVIATION < 30))	rgbLedColor(c5);
		if((DEVIATION > 20) && (DEVIATION < 25))	rgbLedColor(c6);
		if((DEVIATION > 10) && (DEVIATION < 10))	rgbLedColor(c7);
		if((DEVIATION <= 10))	rgbLedColor(c8);

		if(desiredFlag == 1)
		{
			DESIRED = orientation;
			desiredFlag = 0;
		}

	}

	void rgbLedColor(int R, int G, int B)
	{
		TIM3->CCR1 = R;	//R
		TIM3->CCR2 = G;	//G
		TIM3->CCR3 = B;	//B
	}

	float heightFromPressure(float pressure, float pressure0)
	{
		float height, height0;
		height0 = 8;
		int multiplier = 1000;
		height = 0-log(pressure/pressure0)*height0*multiplier;

		return height;
	}

	void setDesiredAzimuth(void)
	{
		desiredFlag = 1;
	}
